/*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*\
|          Copyright (C) 2013 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |
                                                                                
     This program is free software: you can redistribute it and/or modify       
     it under the terms of the GNU General Public License version 2 as          
     published by the Free Software Foundation.                                 
                                                                                
     This program is distributed in the hope that it will be useful,            
     but WITHOUT ANY WARRANTY; without even the implied warranty of             
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              
     GNU General Public License for more details.                               
                                                                                
     You should have received a copy of the GNU General Public License          
     along with this program.  If not, see <http://www.gnu.org/licenses/>.      
                                                                                
            Jack Elston                                                         
|           elstonj@colorado.edu                                               |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/
#include "interface.h"

Interface::Interface(String name) : pThread(name) {
}

bool Interface::init(ShmStruct * shmStructPtr)
{
  bool retval = pThread::init(shmStructPtr);

	initscr();		           /* Start curses mode             */
	curs_set(0);             /* Hide the cursor               */
	//start_color();           /* Start the color functionality */
	cbreak();			           /* Line buffering disabled       */
	keypad(stdscr, TRUE);
	noecho();
	touchwin(stdscr);

	/* Base Window */
	mvaddstr(0,20,"Piccolo CAN <-> Simulink Interface v0.1");
	mvaddstr(23,60,"(c) 2009 Jack Elston");
	refresh();
	/* Communications Window */
	comm_window = addWindow(0,2,60,5,"Communications");
	mvwaddstr(comm_window,1,4,"Device");
	mvwaddstr(comm_window,1,16,"Status");
	mvwaddstr(comm_window,1,44,"Rx");
	mvwaddstr(comm_window,1,53,"Tx");
	wrefresh(comm_window);
	/* AP Output Window */
	apout_window = addWindow(60,2,20,21,"AP Output");
	mvwaddstr(apout_window,1,2,"Ch");
	mvwaddstr(apout_window,1,6,"Angle");
	mvwaddstr(apout_window,1,13,"Pulse");
	char channel[3];
	for(int i=0; i<16; i++) {
		sprintf(channel,"%d",i);
		mvwaddstr(apout_window,i+2,2,channel);
	}
	wrefresh(apout_window);
	/* Configuration Window */
	config_window = addWindow(0,7,60,5,"Configuration");
	mvwaddstr(config_window,1,2,"USB CAN     Piccolo       Conf");
	mvwaddstr(config_window,2,2,"UDP Socket  Host                  Port");
	//mvwaddstr(config_window,3,2,"Simulation       Rx             Tx             RT");
	mvwaddstr(config_window,3,2,"Rates   S       C       A       I       E       G     ");
	wrefresh(config_window);
	/* AP Input Window */
	apin_window = addWindow(0,12,60,11,"AP Input");
	mvwaddstr(apin_window,1,4,"Pressure");
	mvwaddstr(apin_window,2,2,"Dyn");
	mvwaddstr(apin_window,3,2,"Off");
	mvwaddstr(apin_window,4,2,"Stat");
	mvwaddstr(apin_window,5,4,"Temp");
	mvwaddstr(apin_window,6,2,"OAT");
	mvwaddstr(apin_window,8,4,"Engine");
	mvwaddstr(apin_window,9,2,"RPM");
	mvwaddstr(apin_window,1,20,"Gyros");
	mvwaddstr(apin_window,2,18,"roll");
	mvwaddstr(apin_window,3,18,"pitch");
	mvwaddstr(apin_window,4,18,"yaw");
	mvwaddstr(apin_window,6,20,"Accels");
	mvwaddstr(apin_window,7,18,"x");
	mvwaddstr(apin_window,8,18,"y");
	mvwaddstr(apin_window,9,18,"z");
	mvwaddstr(apin_window,1,39,"GPS");
	mvwaddstr(apin_window,3,37,"Lat");
	mvwaddstr(apin_window,4,37,"Lon");
	mvwaddstr(apin_window,5,37,"Height");
	mvwaddstr(apin_window,6,39,"Velocity");
	mvwaddstr(apin_window,7,37,"North");
	mvwaddstr(apin_window,8,37,"East");
	mvwaddstr(apin_window,9,37,"Down");
	wrefresh(apin_window);

	return retval;
}

Interface::~Interface() {
	endwin();
}

void Interface::update() {

	reverseData();

	char tempstr[80];
	mvwaddstr(comm_window,2,2,"USB CAN");
	mvwaddstr(comm_window,2,14,"                    ");
	mvwaddstr(comm_window,2,14,shmPtr->usbcan_status.state.c_str());
	sprintf(tempstr,"%d",shmPtr->usbcan_status.rx);
	mvwaddstr(comm_window,2,41,tempstr);
	sprintf(tempstr,"%d",shmPtr->usbcan_status.tx);
	mvwaddstr(comm_window,2,50,tempstr);

	mvwaddstr(comm_window,3,2,"UDP Socket");
	mvwaddstr(comm_window,3,14,"                    ");
	mvwaddstr(comm_window,3,14,shmPtr->udpsocket_status.state.c_str());
	sprintf(tempstr,"%d",shmPtr->udpsocket_status.rx);
	mvwaddstr(comm_window,3,41,tempstr);
	sprintf(tempstr,"%d",shmPtr->udpsocket_status.tx);
	mvwaddstr(comm_window,3,50,tempstr);
	
	wrefresh(comm_window);

	for(int i=0; i<16; i++) {
		if(i==2) // throttle
			sprintf(tempstr,"%0.2f",(float)(piccolo_tx.servos[i].angle)/10000);
		else
			sprintf(tempstr,"%0.2f",(float)(piccolo_tx.servos[i].angle)/10000*180.0/M_PI);
		mvwaddstr(apout_window,i+2,6,"     ");
		mvwaddstr(apout_window,i+2,6,tempstr);
		sprintf(tempstr,"%d",piccolo_tx.servos[i].pulse);
		mvwaddstr(apout_window,i+2,14,"     ");
		mvwaddstr(apout_window,i+2,14,tempstr);
	}

	wrefresh(apout_window);

	sprintf(tempstr,"%0.2f",piccolo_rx.air.dynamicp);
	mvwaddstr(apin_window,2,8,"        ");
	mvwaddstr(apin_window,2,8,tempstr);
	sprintf(tempstr,"%0.2f",piccolo_rx.air.offsetp);
	mvwaddstr(apin_window,3,8,"        ");
	mvwaddstr(apin_window,3,8,tempstr);
	sprintf(tempstr,"%0.2f",piccolo_rx.air.staticp);
	mvwaddstr(apin_window,4,8,"        ");
	mvwaddstr(apin_window,4,8,tempstr);

	sprintf(tempstr,"%0.2f",piccolo_rx.air.oat);
	mvwaddstr(apin_window,6,8,"      ");
	mvwaddstr(apin_window,6,8,tempstr);

	sprintf(tempstr,"%d",piccolo_rx.engine.lrpm);
	mvwaddstr(apin_window,9,8,"      ");
	mvwaddstr(apin_window,9,8,tempstr);

	sprintf(tempstr,"%0.2f",(float)(piccolo_rx.imu.rates[0])*300/32768.0);
	mvwaddstr(apin_window,2,25,"           ");
	mvwaddstr(apin_window,2,25,tempstr);
	sprintf(tempstr,"%0.2f",(float)(piccolo_rx.imu.rates[1])*300/32768.0);
	mvwaddstr(apin_window,3,25,"           ");
	mvwaddstr(apin_window,3,25,tempstr);
	sprintf(tempstr,"%0.2f",(float)(piccolo_rx.imu.rates[2])*300/32768.0);
	mvwaddstr(apin_window,4,25,"           ");
	mvwaddstr(apin_window,4,25,tempstr);

	sprintf(tempstr,"%0.2f",(float)(piccolo_rx.imu.accelerations[0])*98/32768.0);
	mvwaddstr(apin_window,7,25,"           ");
	mvwaddstr(apin_window,7,25,tempstr);
	sprintf(tempstr,"%0.2f",(float)(piccolo_rx.imu.accelerations[1])*98/32768.0);
	mvwaddstr(apin_window,8,25,"           ");
	mvwaddstr(apin_window,8,25,tempstr);
	sprintf(tempstr,"%0.2f",(float)(piccolo_rx.imu.accelerations[2])*98/32768.0);
	mvwaddstr(apin_window,9,25,"           ");
	mvwaddstr(apin_window,9,25,tempstr);

	sprintf(tempstr,"%d/%d/%d %d:%d:%0.2f",
			piccolo_rx.gps.month,
			piccolo_rx.gps.day,
			piccolo_rx.gps.year,
			piccolo_rx.gps.hour,
			piccolo_rx.gps.minute,
			(float)(piccolo_rx.gps.second) + (float)(piccolo_rx.gps.frac_sec)/100.0);
	mvwaddstr(apin_window,2,37,"                      ");
	mvwaddstr(apin_window,2,37,tempstr);

	sprintf(tempstr,"%0.2f",((float)(piccolo_rx.gps.lat))/1000.0/3600.0);
	mvwaddstr(apin_window,3,45,"        ");
	mvwaddstr(apin_window,3,45,tempstr);
	sprintf(tempstr,"%0.2f",((float)(piccolo_rx.gps.lon))/1000.0/3600.0);
	mvwaddstr(apin_window,4,45,"        ");
	mvwaddstr(apin_window,4,45,tempstr);
	sprintf(tempstr,"%0.2f",(float)(piccolo_rx.gps.height)/100.0);
	mvwaddstr(apin_window,5,45,"        ");
	mvwaddstr(apin_window,5,45,tempstr);

	sprintf(tempstr,"%0.2f",(float)(piccolo_rx.gps.velocity[0])/100.0);
	mvwaddstr(apin_window,7,45,"        ");
	mvwaddstr(apin_window,7,45,tempstr);
	sprintf(tempstr,"%0.2f",(float)(piccolo_rx.gps.velocity[1])/100.0);
	mvwaddstr(apin_window,8,45,"        ");
	mvwaddstr(apin_window,8,45,tempstr);
	sprintf(tempstr,"%0.2f",(float)(piccolo_rx.gps.velocity[2])/100.0);
	mvwaddstr(apin_window,9,45,"        ");
	mvwaddstr(apin_window,9,45,tempstr);

	wrefresh(apin_window);

	sprintf(tempstr,"%d",shmPtr->usbcan_status.apAddress);
	mvwaddstr(config_window,1,22,tempstr);
	sprintf(tempstr,"%x",piccolo_tx.config.config);
	mvwaddstr(config_window,1,33,tempstr);
	mvwaddstr(config_window,2,19,shmPtr->udpsocket_status.hostStr.c_str());
	mvwaddstr(config_window,2,41,shmPtr->udpsocket_status.portStr.c_str());

	sprintf(tempstr,"%0.1f",shmPtr->packet_status.servo.rate);
	mvwaddstr(config_window,3,12,"     ");
	mvwaddstr(config_window,3,12,tempstr);
	sprintf(tempstr,"%0.1f",shmPtr->packet_status.conf.rate);
	mvwaddstr(config_window,3,20,"     ");
	mvwaddstr(config_window,3,20,tempstr);
	sprintf(tempstr,"%0.1f",shmPtr->packet_status.air.rate);
	mvwaddstr(config_window,3,28,"     ");
	mvwaddstr(config_window,3,28,tempstr);
	sprintf(tempstr,"%0.1f",shmPtr->packet_status.imu.rate);
	mvwaddstr(config_window,3,36,"     ");
	mvwaddstr(config_window,3,36,tempstr);
	sprintf(tempstr,"%0.1f",shmPtr->packet_status.engine.rate);
	mvwaddstr(config_window,3,44,"     ");
	mvwaddstr(config_window,3,44,tempstr);
	sprintf(tempstr,"%0.1f",shmPtr->packet_status.gps.rate);
	mvwaddstr(config_window,3,52,"     ");
	mvwaddstr(config_window,3,52,tempstr);
	
	/*sprintf(tempstr,"%d",piccolo_rx.imu.sequence);
	mvwaddstr(config_window,3,14,"   ");
	mvwaddstr(config_window,3,14,tempstr);
	sprintf(tempstr,"%d",piccolo_rx.engine.time);
	mvwaddstr(config_window,3,22,"          ");
	mvwaddstr(config_window,3,22,tempstr);
	sprintf(tempstr,"%d",piccolo_tx.config.time);
	mvwaddstr(config_window,3,37,"          ");
	mvwaddstr(config_window,3,37,tempstr);*/
	wrefresh(config_window);
}

WINDOW * Interface::addWindow(int x, int y, int size_x, int size_y, String title) {
	WINDOW * window = newwin(size_y,size_x,y,x);
	box(window, 0, 0);

	char my_title[128];
	sprintf(my_title, "[ %s ]", title.c_str());
	int stringsize = strlen(my_title);
	mvwaddstr(window, 0, 2, my_title);

	touchwin(window);
	wrefresh(window);

	return window;
}

void Interface::reverseData()
{
	uint8_t * from, * to;
	from = (uint8_t*)(&(shmPtr->piccolo_rx));
	to = (uint8_t*)(&piccolo_rx);

	revBytes(&from,&to,sizeof(float));    //dynamicp
  revBytes(&from,&to,sizeof(float));    //oat;
  revBytes(&from,&to,sizeof(float));    //offsetp;
  revBytes(&from,&to,sizeof(float));    //staticp;
  revBytes(&from,&to,sizeof(int16_t));  //rates[3];
  revBytes(&from,&to,sizeof(int16_t));  //rates[3];
  revBytes(&from,&to,sizeof(int16_t));  //rates[3];
  revBytes(&from,&to,sizeof(int16_t));  //accelerations[3];
  revBytes(&from,&to,sizeof(int16_t));  //accelerations[3];
  revBytes(&from,&to,sizeof(int16_t));  //accelerations[3];
  revBytes(&from,&to,sizeof(uint8_t));  //sequence;
  revBytes(&from,&to,sizeof(uint16_t)); //lrpm;
  revBytes(&from,&to,sizeof(uint16_t)); //rrpm;
  revBytes(&from,&to,sizeof(uint32_t)); //time_rx;
  revBytes(&from,&to,sizeof(uint8_t));  //month;
  revBytes(&from,&to,sizeof(uint8_t));  //day;
  revBytes(&from,&to,sizeof(uint16_t)); //year;
  revBytes(&from,&to,sizeof(uint8_t));  //hour;
  revBytes(&from,&to,sizeof(uint8_t));  //minute;
  revBytes(&from,&to,sizeof(uint8_t));  //second;
  revBytes(&from,&to,sizeof(uint8_t));  //frac_sec;
  revBytes(&from,&to,sizeof(uint32_t)); //lat;
  revBytes(&from,&to,sizeof(uint32_t)); //lon;
  revBytes(&from,&to,sizeof(uint32_t)); //height;
  revBytes(&from,&to,sizeof(int16_t));  //velocity[3];
  revBytes(&from,&to,sizeof(int16_t));  //velocity[3];
  revBytes(&from,&to,sizeof(int16_t));  //velocity[3];

	from = (uint8_t*)(&(shmPtr->piccolo_tx));
	to = (uint8_t*)(&piccolo_tx);

	for(int i=0; i<16; i++) {
		revBytes(&from,&to,sizeof(uint16_t)); //pulses;
		revBytes(&from,&to,sizeof(int16_t));  //angles;
	}
	revBytes(&from,&to,sizeof(uint32_t));   //time_tx;
	revBytes(&from,&to,sizeof(uint32_t));   //config;
}

void Interface::revBytes(uint8_t ** from, uint8_t ** to, int num)
{
	for(int i=0; i<num; i++)
		(*to)[i] = (*from)[num-i-1];
	(*to) = (*to) + num;
	(*from) = (*from) + num;
}
